#include "ros/ros.h"
#include <iostream>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <string>
#include <iostream>
#include "conf.h"
#include "min.h"
#include "layer2.h"
#include "std_msgs/String.h"//ROS自定义的数据类型String所在的头文件
#include "std_msgs/Float64MultiArray.h"//ROS自定义的数据类型String所在的头文件
#include "std_msgs/Float32MultiArray.h"//ROS自定义的数据类型String所在的头文件
#include <pthread.h>
#include "std_msgs/Int16MultiArray.h"
#include "navigation_msg/odom.h"
#include "std_msgs/Int8.h"//ROS自定义的数据类型String所在的头文件
pthread_t uart_thread;
pthread_t ladar_thread;
using namespace std;

extern small_car_pulse Small_Car_Pulse;
extern int8_t ladar_massage_mark;
extern int8_t sensor_massage_mark;
extern int8_t is_hitting;

void commandCallback(const std_msgs::Int16MultiArray command_topic)
{
   //printf("hello0000000\r\n") ;

   /*printf("command_array: %d,%d,%d,%d,%d\r\n",command_topic.data[0],command_topic.data[1],
           command_topic.data[2],command_topic.data[3],command_topic.data[4]);*/

   motion(command_topic.data[0],command_topic.data[1],command_topic.data[2],
           command_topic.data[3],command_topic.data[4]);

}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "serial_uart_node");
  ros::NodeHandle serial_handle;



#if 1
    if (pthread_create(&uart_thread,NULL, pthread_uart_monitor,NULL)!=0) {
                printf("Create  pthread_uart_monitor error!\n");
                exit(1);
    }
    sleep(1);
#endif

#if 1
        if (pthread_create(&ladar_thread,NULL, pthread_ladar,NULL)!=0) {
            printf("Create cameras_thread error!\n");
            exit(1);
        }
       // printf("ladar_thread id is: %u.\n",(unsigned int)ladar_thread);
        pthread_detach(ladar_thread);
        sleep(1);
#endif


  ros::Rate loop_rate(10000);
  ros::Subscriber rev_com = serial_handle.subscribe("key_board/topic_command", 10, commandCallback);

  ros::Publisher pub_odom_data  = serial_handle.advertise<navigation_msg::odom>
          ("serial/odom_data", 10);


  ros::Publisher ladar_distance_data  = serial_handle.advertise<std_msgs::Int8>
          ("serial/ladar_distance_data", 10);




  //ros::param::set("bool_param",false);
  //ros::param::set("turn_enable",test_i);

  while (serial_handle.ok())
  {
      navigation_msg::odom odom_message;
      std_msgs::Int8 val_ladar_distance;

      if(sensor_massage_mark==1){

          odom_message.left_speed=Small_Car_Pulse.left_speed;
          odom_message.right_speed=Small_Car_Pulse.right_speed;
          odom_message.car_angle=Small_Car_Pulse.car_angle;
          odom_message.motor_pulse=Small_Car_Pulse.motor_pulse;
          //odom_message.imu_yaw=Small_Car_Pulse.imu_yaw;
          pub_odom_data.publish(odom_message);
          sensor_massage_mark=0;
      }

      if(ladar_massage_mark==1){

          val_ladar_distance.data = is_hitting;
          ladar_distance_data.publish(val_ladar_distance);
          ladar_massage_mark=0;
      }

      loop_rate.sleep();
      ros::spinOnce();
  }

  return 0;
}




